#ifndef I_LIB_PC_I_GPD_S_H
#define I_LIB_PC_I_GPD_S_H
/*
Author: Wang Liang (wangliang18@baidu.com)

Data structures and functions for ground plane detection (gpd)
Note: Functions this files in this file assume the LiDAR points are in the coordinate system
defined by the Velodyne-64 sensor:
Up/down: Z
Front/back: X
Left/right: Y
The unit of size/distance relatd parameters used in this file is meter 
*/

#include <vector>
#include <cassert>
#include "../core/i_blas.h"
#include "../algorithm/i_sort.h"
#include "../geometry/i_plane.h"
#include "i_struct_s.h"

namespace idl
{
    template<typename T>
    struct GroundPlane
    {
        T params[4];
        GroundPlane(){ i_zero4(params); }
        ~GroundPlane(){};
        GroundPlane& operator=(const GroundPlane<T>& pi)
        {
            i_copy4(pi.params, this->params);
            return(*this);
        }
        T get_degree_normal_to_z() const
        {
            T normal[3];
            i_copy3(params, normal);
            if (normal[2] < (T)0.0)
            {
                i_neg3(normal);
            }
            /*normalization*/
            i_scale3(normal, i_rec(i_sqrt(i_squaresum3(normal))));
            return i_radians_to_degree(i_acos(normal[2]));
        }
        void force_positive_normal_z()
        {
            if (params[2] < (T)0.0)
            {
                i_neg4(params);
            }
        }
        void force_invalid()
        {
            i_zero4(params);
        }
        bool is_valid() const
        {
            if (params[0] != (T)0.0 ||
                params[1] != (T)0.0 ||
                params[2] != (T)0.0 ||
                params[3] != (T)0.0)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
    };

    template <typename DATA_TYPE>
    class MultiScaleGroundPlaneDetector
    {
        static const unsigned int _nr_filter_sample_points = 32;
        static const unsigned int _nr_ransac_max_sample_points = 512;
        static const unsigned int _nr_ransac_max_hypothesis = 32;
        static const short _invalid_signed_ground_distance = 32639;
        /*defined in millimeter 150mm == 0.150m*/
        static const short _max_z_dim_diff_ground_candidates = 150;
    public:
        MultiScaleGroundPlaneDetector();
        
        ~MultiScaleGroundPlaneDetector();

        int detect(const VoxelGridXYPyramid<DATA_TYPE>& vgp, 
                   DATA_TYPE upper_bound_z, 
                   DATA_TYPE lower_bound_z);

        const char* get_const_labels() const 
        { 
            return _labels; 
        }

        void set_dist_tol(double value)
        { 
            _dist_tol = value; 
        }
        
        void set_nr_inliers_tol(unsigned int value)
        { 
            _nr_inliers_tol = value; 
        }
        
        void set_nr_samples_tol(unsigned int value)
        { 
            _nr_samples_tol = value;
        }
        
        unsigned int max_nr_points() const 
        { 
            return _nr_points_max;
        }

        unsigned int set_nr_points_max(unsigned int size)
        {
            return reallocate_data(size);
        }

        /*returns the signed to ground distance of the ith point in millimeter*/
        short invalid_ground_distance_in_mm() const
        {
            return (_invalid_signed_ground_distance);
        }

        short get_signed_ground_distance_in_mm(unsigned int i) const
        {
            if (i >= _nr_points_max)
            {
                return 0;
            }
            return (_signed_ground_dists_in_mm[i]);
        }

        /*returns the signed to ground distance of the ith point in millimeter*/
        double get_signed_ground_distance_in_meter(unsigned int i) const
        {
            if (i >= _nr_points_max)
            {
                return 0;
            }
            return ((double)_signed_ground_dists_in_mm[i] * 1e-3);
        }
    
        /* add by Fangzhen Li on 03/27/17 */
        bool get_voxel_ground_parems(
                const VoxelGridXYPyramid<DATA_TYPE>& vgp,
                const DATA_TYPE *pt,
                unsigned int nr_point_element,
                DATA_TYPE *params); 
    private:
        int filter_voxel(const Voxel<DATA_TYPE>& voxel, 
                             const DATA_TYPE* pc,
                             unsigned int nr_points,
                             unsigned int nr_point_element,
                             DATA_TYPE upper_bound_z,
                             DATA_TYPE lower_bound_z,
                             DATA_TYPE thre);
        
        int filter_voxelgrid(const VoxelGridXY<DATA_TYPE>& vg, 
            DATA_TYPE upper_bound_z,
            DATA_TYPE lower_bound_z, 
            DATA_TYPE thre);

        int fit_voxel(const Voxel<DATA_TYPE>& voxel,
            const DATA_TYPE* pc,
            GroundPlane<DATA_TYPE>& ground_plane,
            unsigned int nr_points,
            unsigned int nr_point_element,
            DATA_TYPE upper_bound_z,
            DATA_TYPE lower_bound_z,
            DATA_TYPE dist_tol);

        unsigned int reallocate_data(unsigned int size);
        bool check_gps_buffer_dims(unsigned int nr_actual_hor, unsigned int nr_actual_ver);
        
        void calc_signed_ground_dist_row(const VoxelGridXY<DATA_TYPE>& vg,
                                         const DATA_TYPE *pc,
                                         unsigned int nr_points,
                                         unsigned int nr_point_element,
                                         unsigned int row);

        void calc_signed_ground_dist_col(const VoxelGridXY<DATA_TYPE>& vg,
                                         const DATA_TYPE *pc,
                                         unsigned int nr_points,
                                         unsigned int nr_point_element,
                                         unsigned int col);

        void calc_signed_ground_dist(const VoxelGridXY<DATA_TYPE>& vg,
                                     const DATA_TYPE* pc,
                                     unsigned int nr_points,
                                     unsigned int nr_point_element);

        int fit_plane(const DATA_TYPE* pc,
            unsigned int nr_point_element,
            const std::vector<int>& indices,
            GroundPlane<DATA_TYPE>& plane,
            DATA_TYPE dist_tol);

    private:
        unsigned int _nr_inliers_tol, _nr_samples_tol, _nr_points_max;
        unsigned int _nr_max_underground_points;
        unsigned int _nr_gpfit_voxels_vertical;
        unsigned int _nr_gpfit_voxels_horizontal;
        double _dist_tol, _dist_underground_tol, _normal_degree_tol;
        char *_labels;
        /*added by Liang on 09/28/16*/
        /*Note: store value in millimeter to reduce memory usage*/
        short *_signed_ground_dists_in_mm; 
        GroundPlane<DATA_TYPE> **_gps;
        DATA_TYPE *_pf_threeds;
    };

    template <typename DATA_TYPE>
    MultiScaleGroundPlaneDetector<DATA_TYPE>::MultiScaleGroundPlaneDetector() : 
        _nr_inliers_tol(20),
        _nr_samples_tol(32),
        _dist_tol(0.06), /*default 6 cm*/
        _dist_underground_tol(0.5), /*default 50cm*/
        _normal_degree_tol(30.0), /*default 30 degree*/
        _nr_max_underground_points(100000),
        _nr_points_max(320000),
        _nr_gpfit_voxels_vertical(16),
        _nr_gpfit_voxels_horizontal(16)
    {
        _labels = i_alloc<char>(_nr_points_max);
        _pf_threeds = i_alloc_aligned<DATA_TYPE>((_nr_ransac_max_sample_points << 2), 4);
        _signed_ground_dists_in_mm = i_alloc<short>(_nr_points_max);
        _gps = i_alloc2<GroundPlane<DATA_TYPE> >(
               _nr_gpfit_voxels_vertical, _nr_gpfit_voxels_horizontal);
        assert(_labels != NULL);
        assert(_signed_ground_dists_in_mm != NULL);
        assert(_gps != NULL); 
        assert(_pf_threeds != NULL);
        assert(i_verify_alignment(_pf_threeds, 4));
    }

    template <typename DATA_TYPE>
    MultiScaleGroundPlaneDetector<DATA_TYPE>::~MultiScaleGroundPlaneDetector()
    {
        i_free<char>(_labels);
        i_free<short>(_signed_ground_dists_in_mm);
        i_free2<GroundPlane<DATA_TYPE> >(_gps);
        i_free_aligned<DATA_TYPE>(_pf_threeds);
    }

    template <typename DATA_TYPE>
    int MultiScaleGroundPlaneDetector<DATA_TYPE>::filter_voxel(const Voxel<DATA_TYPE>& voxel,
                                                               const DATA_TYPE* pc,
                                                               unsigned int nr_points_total,
                                                               unsigned int nr_point_element,
                                                               DATA_TYPE upper_bound_z,
                                                               DATA_TYPE lower_bound_z,
                                                               DATA_TYPE thre)
    {
        unsigned int i, j, nr, nr_points = voxel.nr_points(), nr_samples = 0;
        int pos, sampled_indices[_nr_filter_sample_points];
        int rseed = I_DEFAULT_SEED;
        DATA_TYPE z_ref, dz, sampled_z[_nr_filter_sample_points];
        /*needs at least two points to do z comparision*/
        if (nr_points <= 1)
        {
            return (-1);
        }
        else if (nr_points <= _nr_filter_sample_points)
        {
            i_ramp(sampled_indices, nr_points);
        }
        else
        {
            i_random_sample(sampled_indices, (int)_nr_filter_sample_points, (int)nr_points, rseed);
        }

        nr_samples = i_min(_nr_filter_sample_points, nr_points);

        /*hardcoded threshold:*/
        /*inliers ratio >= 1/16*/
        unsigned int nr_tol_inlier = i_max((unsigned int)1, (nr_samples >> 4));
        /*outliers ratio < 1/4*/
        unsigned int nr_tol_outlier = (nr_samples >> 2);

        for (i = 0; i < nr_samples; ++i)
        {
            pos = voxel._indices[sampled_indices[i]];
            assert(pos < (int)nr_points_total);
            /*requires the Z element is in the third position, i.e., after X, Y*/
            sampled_z[i] = (pc + (pos * nr_point_element))[2];
        }

        for (i = 0; i < nr_points; ++i)
        {
            nr = 0;
            pos = voxel._indices[i];
            assert(pos < (int)nr_points_total);
            /*requires that the Z element to be in the third position, after X, Y*/
            z_ref = (pc + (pos * nr_point_element))[2];
            if (z_ref <= lower_bound_z || z_ref >= upper_bound_z)
            {
                continue;
            }
            for (j = 0; j < nr_samples; ++j)
            {
                dz = i_abs(sampled_z[j] - z_ref);
                if (dz > thre)
                {
                    nr++;
                }
                /*no need to further check*/
                if (nr > nr_tol_outlier)
                {
                    break;
                }
            }
            /*all the points have very similar z*/
            if (nr <= nr_tol_inlier)
            {
                _labels[pos] = 1;
            }
            else
            {
                /*voxel has have very distinctive z*/
                if (nr > nr_tol_outlier)
                {
                    _labels[pos] = -1;
                }
            }
        }
        return (0);
    }

    template <typename DATA_TYPE>
    int MultiScaleGroundPlaneDetector<DATA_TYPE>::filter_voxelgrid(const VoxelGridXY<DATA_TYPE>& vg,
                                                                       DATA_TYPE upper_bound_z,
                                                                       DATA_TYPE lower_bound_z,
                                                                       DATA_TYPE thre)
    {
        unsigned int i;
        if (vg.nr_points() > _nr_points_max)
        {
            return (-1);
        }
        if (!_labels || !vg.const_data())
        {
            return (-1);
        }
        for (i = 0; i < vg.nr_voxel(); ++i)
        {
            filter_voxel(vg[i], vg.const_data(), vg.nr_points(), vg.nr_point_element(),
                         upper_bound_z, lower_bound_z, thre);
        }
        return (0);
    }

    template <typename DATA_TYPE>
    bool MultiScaleGroundPlaneDetector<DATA_TYPE>::check_gps_buffer_dims
        (unsigned int nr_actual_hor, unsigned int nr_actual_ver)
    {
        if (nr_actual_hor == _nr_gpfit_voxels_horizontal &&
            nr_actual_ver == _nr_gpfit_voxels_vertical)
        {
            return (true);
        }
        /*resize the buffer for recording ground plane equations*/
        if (nr_actual_ver != _nr_gpfit_voxels_vertical ||
            nr_actual_hor != _nr_gpfit_voxels_horizontal)
        {
            i_free2<GroundPlane<DATA_TYPE> >(_gps);
            _gps = i_alloc2<GroundPlane<DATA_TYPE> >(
                nr_actual_ver, nr_actual_hor);
            if (!_gps)
            {
                return (false);
            }
            _nr_gpfit_voxels_vertical = nr_actual_ver;
            _nr_gpfit_voxels_horizontal = nr_actual_hor;
        }
        return (true);
    }

    /*return the new "max # of points"*/
    template <typename DATA_TYPE>
    unsigned int MultiScaleGroundPlaneDetector<DATA_TYPE>::reallocate_data(unsigned int size)
    {
        if (_nr_points_max >= size) 
        {
            return (_nr_points_max);
        }

        _nr_points_max = size; 
        
        if (_labels) 
        {
            i_free<char>(_labels);
        }

        if (_signed_ground_dists_in_mm)
        {
            i_free<short>(_signed_ground_dists_in_mm);
        }

        _labels = i_alloc<char>(_nr_points_max);
        _signed_ground_dists_in_mm = i_alloc<short>(_nr_points_max);
        
        if (!_labels || !_signed_ground_dists_in_mm)
        {
            i_free<char>(_labels);
            i_free<short>(_signed_ground_dists_in_mm);
            return (0);
        }

        return (_nr_points_max);
    }

    template <typename DATA_TYPE>
    int MultiScaleGroundPlaneDetector<DATA_TYPE>::fit_plane(
        const DATA_TYPE* pc,
        unsigned int nr_point_element,
        const std::vector<int>& indices,
        GroundPlane<DATA_TYPE>& plane,
        DATA_TYPE dist_tol)
    {
        /*variables*/
        unsigned int i, i3, j, indices_size = indices.size();
        unsigned int nr_samples = 0;
        int ransac_indices[_nr_ransac_max_sample_points];
        const unsigned int max_nr_points = _nr_ransac_max_sample_points;
        const unsigned int nr_ransac_trials = _nr_ransac_max_hypothesis;

        /*each hypothesis needs 3 sample points*/
        const unsigned int nr_needed_randsamples = (_nr_ransac_max_hypothesis * 3);
        int indices_trial[nr_needed_randsamples];
        int rseed = I_DEFAULT_SEED;

        DATA_TYPE pt_to_pl_dist;
        /*store: x, y, z, x, y, z, x, y, z*/
        DATA_TYPE pt_samples[9];
        DATA_TYPE *p_src = NULL;
        DATA_TYPE *p_dst = NULL;
        GroundPlane<DATA_TYPE> hypothesis[_nr_ransac_max_hypothesis];
        int nr_inliers, nr_inliers_terminate, nr_inliers_best = 0, best = -1;

        /*by default set to invalid*/
        plane.force_invalid();

        /*check if enough samples*/
        if (indices_size < _nr_samples_tol)
        {
            return (-1); /*not enough samples, failed and return*/
        }
        else if (indices_size > max_nr_points)
        {
            i_random_sample(ransac_indices, max_nr_points, (int)indices_size, rseed);
            for (i = 0; i < max_nr_points; ++i)
            {
                ransac_indices[i] = indices[ransac_indices[i]];
            }
        }
        else
        {
            i_copy(indices.data(), ransac_indices, indices_size);
        }

        /*actual number of samples used for plane fitting*/
        nr_samples = i_min(max_nr_points, indices_size);
        /*0.99 inliers confidence level, assume ransac can break 
        - speedup the process, 0.99 is high enough to guarantee the
        best plane candidate is selected*/
        nr_inliers_terminate = i_round((int)nr_samples * 0.99);

        /*copy 3D points*/
        p_dst = _pf_threeds;

        for (i = 0; i < nr_samples; ++i, p_dst += 4)
        {
            i_copy3(pc + (nr_point_element * ransac_indices[i]), p_dst);
        }

        /*generate random samples - indices*/
        if (nr_samples >= nr_needed_randsamples)
        {
            i_random_sample(indices_trial, nr_needed_randsamples, nr_samples, rseed);
            /*apply a random shuffle because the generated random numbers are sorted*/
            i_randomized_shuffle1(indices_trial, nr_needed_randsamples, rseed);
        }
        else
        {
            for (i = 0; i < nr_needed_randsamples; i += 3)
            {
                i_random_sample(indices_trial + i, 3, nr_samples, rseed);
            }
        }

        /*vote for plane hypothesis*/
        for (i = 0, i3 = 0; i < nr_ransac_trials; ++i, i3 += 3)
        {
            nr_inliers = 0;
            i_copy3(_pf_threeds + (indices_trial[i3] << 2), pt_samples);
            i_copy3(_pf_threeds + (indices_trial[i3 + 1] << 2), pt_samples + 3);
            i_copy3(_pf_threeds + (indices_trial[i3 + 2] << 2), pt_samples + 6);
            i_plane_fit_destroyed(pt_samples, hypothesis[i].params);
            /*check if the plane hypothesis has valid geometry*/
            if (hypothesis[i].get_degree_normal_to_z() > (DATA_TYPE)_normal_degree_tol)
            {
                continue;
            }
            /*iterate samples and check if the point to plane distance is within threshold*/
            p_src = _pf_threeds;
            for (j = 0; j < nr_samples; ++j, p_src += 4)
            {
                pt_to_pl_dist = i_plane_to_point_distance_w_unit_norm(
                    hypothesis[i].params, p_src);

                if (pt_to_pl_dist < dist_tol)
                {
                    nr_inliers++;
                }
            }
            /*record the best plane index*/
            if (nr_inliers > nr_inliers_best)
            {
                nr_inliers_best = nr_inliers;
                best = (int)i;
                
                if (nr_inliers_best >= nr_inliers_terminate)
                {
                    break; /*enough inliers -- terminate the ransac*/
                }
            }
        }

        /*if not pass the minimal number of inliers request or
        no valid geometry*/
        if (nr_inliers_best < (int)_nr_inliers_tol || best == -1)
        {
            return (-1); /*not enough inliers, failed and return*/
        }

        /*assign the best plane*/
        plane = hypothesis[best];

        /*iterate samples and check if the point to plane distance is within threshold*/
        p_src = _pf_threeds;
        p_dst = _pf_threeds;

        nr_inliers = 0;

        for (j = 0; j < nr_samples; ++j, p_src += 4)
        {
            pt_to_pl_dist = i_plane_to_point_distance_w_unit_norm(plane.params, p_src);
            if (pt_to_pl_dist < dist_tol)
            {
                i_copy3(p_src, p_dst);
                p_dst += 3;
                nr_inliers++;
            }
        }

        /*note that _pf_threeds will be destroyed after calling this routine*/
        i_plane_fit_total_least_square(_pf_threeds, plane.params, nr_inliers);
        /*filtering: the best plane is not valid*/
        if (plane.get_degree_normal_to_z() > _normal_degree_tol)
        {
            return (-1);
        }
        return (nr_inliers);
    }

    template <typename DATA_TYPE>
    int MultiScaleGroundPlaneDetector<DATA_TYPE>::fit_voxel(const Voxel<DATA_TYPE>& voxel,
        const DATA_TYPE* pc,
        GroundPlane<DATA_TYPE>& ground_plane,
        unsigned int nr_points,
        unsigned int nr_point_element,
        DATA_TYPE upper_bound_z,
        DATA_TYPE lower_bound_z,
        DATA_TYPE dist_tol)
    {
        if (voxel.empty())
        {
            return (0);
        }

        int j;
        unsigned int i;
        unsigned int nr_ground_points = 0;
        //- not counting underneadth ground points 10/16/2017 - Wang Liang
        DATA_TYPE pt_to_plane_dist, pt_to_plane_dist_signed;//, span_thre;
        const DATA_TYPE* cptr_pc = NULL;
        const DATA_TYPE dist_tol_losse = (dist_tol * 2); /*default 20cm*/
        const DATA_TYPE dist_tol_strict = (dist_tol / 2); /*default 5cm*/
        GroundPlane<DATA_TYPE> plane;
        std::vector<int> indices;
        indices.reserve(voxel.nr_points());
        
        /*collect ground candidates*/
        std::vector<int>::const_iterator c_it = voxel._indices.begin();
        while (c_it != voxel._indices.end())
        {
            if (_labels[*c_it] == 1)
            {
                indices.push_back(*c_it);
            }
            ++c_it;
        }

        if (fit_plane(pc, nr_point_element, indices, plane, dist_tol) <= 0)
        {
            return (0);
        }

        /*force the normal to be along the positive side of ground*/
        plane.force_positive_normal_z();

        /*the plane now must be valid, i.e.,
        normal direction, # of inliers, # of samples...*/
        /*assign the best plane to the output*/
        ground_plane = plane;
        
        /*compute centroid in the XY plane*/
        //centroid[0] = voxel._v[0] + voxel._dim_x / 2;
        //centroid[1] = voxel._v[1] + voxel._dim_y / 2;
        //span_thre = i_sqr(i_max(voxel._dim_x, voxel._dim_y) / 2);

        /*record the signed distance from point to ground plane*/
        for (i = 0; i < voxel.nr_points(); ++i)
        {
            j = voxel._indices[i];
            
            assert(j < (int)nr_points);
            cptr_pc = pc + (nr_point_element * j);
            pt_to_plane_dist_signed = 
                i_plane_to_point_signed_distance_w_unit_norm(plane.params, cptr_pc);

            /*added by Liang on 09/28/16*/
            /*note: pt_to_plane_dist_signed in meters*/
            /*_signed_ground_dists_in_mm in mm*/
            _signed_ground_dists_in_mm[j] = (short)(pt_to_plane_dist_signed * (1e+3));

            /*invalid points do not count*/
            if (cptr_pc[2] <= lower_bound_z || cptr_pc[2] >= upper_bound_z)
            {
                continue;
            }
                                                                           
            pt_to_plane_dist = i_abs(pt_to_plane_dist_signed);

            //-not counting underneadth ground points 10 / 16 / 2017 - Wang Liang
            //if (pt_to_plane_dist_signed < -(DATA_TYPE)_dist_underground_tol)
            //{
                //if (i_sqr(cptr_pc[0] - centroid[0]) + 
                //    i_sqr(cptr_pc[1] - centroid[1]) < span_thre)
                //{
                //    nr_underneath_ground++;
                //}
            //}

            if (pt_to_plane_dist >= dist_tol_losse)
            {
                continue;
            }

            if (_labels[j] == 0)
            {
                if (pt_to_plane_dist < dist_tol)
                {
                    _labels[j] = 2;
                    nr_ground_points++;
                }
            }
            else if (_labels[j] == 1)
            {
                if (pt_to_plane_dist < dist_tol_losse)
                {
                    _labels[j] = 2;
                    nr_ground_points++;
                }
            }
            else
            {
                /*case that (_labels[j] == -1)*/
                if (pt_to_plane_dist < dist_tol_strict)
                {
                    _labels[j] = 2;
                    nr_ground_points++;
                }
            }
        }

        /*too many points are underneath the ground 
          probably fitted a plane with the top surface 
          of a car - therefore clear the labels*/
        //- not counting underneadth ground points 10/16/2017 - Wang Liang
        //if (nr_underneath_ground > _nr_max_underground_points)
        //{
        //    for (i = 0; i < voxel.nr_points(); ++i)
        //    {
        //        j = voxel._indices[i];
        //        _labels[j] = 0;
        //        _signed_ground_dists_in_mm[j] = _invalid_signed_ground_distance;
        //    }
        //    /*added by Liang on 09/28/16*/
        //    ground_plane.force_invalid();
        //    nr_ground_points = 0;
        //}

        return (nr_ground_points);
    }

    template <typename DATA_TYPE>
    void MultiScaleGroundPlaneDetector<DATA_TYPE>::calc_signed_ground_dist_row(
        const VoxelGridXY<DATA_TYPE>& vg,
        const DATA_TYPE *pc,
        unsigned int nr_points,
        unsigned int nr_point_element,
        unsigned int row)
    {
        if (row >= _nr_gpfit_voxels_vertical ||
            _nr_gpfit_voxels_horizontal < 2  ||
            pc == NULL)
        {
            return;
        }

        unsigned int nr_half_col = (_nr_gpfit_voxels_horizontal >> 1);
        unsigned int i, j, col, nr_points_voxel;
        const DATA_TYPE* cptr_pc = NULL;
        DATA_TYPE pt_to_plane_dist_signed = (DATA_TYPE)0;
        
        /*left half*/
        for (col = 0; col < nr_half_col; col++)
        {
            if (_gps[row][col].is_valid())
            {
                continue;
            }
            const Voxel<DATA_TYPE> &voxel = vg(row, col);
            nr_points_voxel = voxel.nr_points();
            if (!nr_points_voxel)
            {
                continue; /*empty voxel*/
            }
            /*check if the right neighbor is valid*/
            const GroundPlane<DATA_TYPE> &plane = _gps[row][col + 1];
            if (!plane.is_valid())
            {
                continue;
            }
            for (i = 0; i < nr_points_voxel; ++i)
            {
                j = (unsigned int)voxel._indices[i];
                assert(j < nr_points);
                cptr_pc = pc + (nr_point_element * j);
                pt_to_plane_dist_signed =
                    i_plane_to_point_signed_distance_w_unit_norm(plane.params, cptr_pc);
                _signed_ground_dists_in_mm[j] = 
                    (short)i_min((int)(pt_to_plane_dist_signed * (1e+3)),
                    (int)_signed_ground_dists_in_mm[j]);
            }
        }

        /*right half*/
        for (col = _nr_gpfit_voxels_horizontal - 1; col >= nr_half_col; col--)
        {
            if (_gps[row][col].is_valid())
            {
                continue;
            }
            const Voxel<DATA_TYPE> &voxel = vg(row, col);
            nr_points_voxel = voxel.nr_points();
            if (!nr_points_voxel)
            {
                continue; /*empty voxel*/
            }
            /*check if the left neighbor is valid*/
            const GroundPlane<DATA_TYPE> &plane = _gps[row][col - 1];
            if (!plane.is_valid())
            {
                continue;
            }
            for (i = 0; i < nr_points_voxel; ++i)
            {
                j = (unsigned int)voxel._indices[i];
                assert(j < nr_points);
                cptr_pc = pc + (nr_point_element * j);
                pt_to_plane_dist_signed =
                    i_plane_to_point_signed_distance_w_unit_norm(plane.params, cptr_pc);
                _signed_ground_dists_in_mm[j] = 
                    (short)i_min((int)(pt_to_plane_dist_signed * (1e+3)),
                    (int)_signed_ground_dists_in_mm[j]);
            }
        }
    }

    template <typename DATA_TYPE>
    void MultiScaleGroundPlaneDetector<DATA_TYPE>::calc_signed_ground_dist_col(
        const VoxelGridXY<DATA_TYPE>& vg,
        const DATA_TYPE *pc,
        unsigned int nr_points,
        unsigned int nr_point_element,
        unsigned int col)
    {
        if (col >= _nr_gpfit_voxels_horizontal ||
            _nr_gpfit_voxels_vertical < 2 ||
            pc == NULL)
        {
            return;
        }

        unsigned int nr_half_row = (_nr_gpfit_voxels_vertical >> 1);
        unsigned int i, j, row, nr_points_voxel;
        const DATA_TYPE* cptr_pc = NULL;
        DATA_TYPE pt_to_plane_dist_signed = (DATA_TYPE)0;

        /*top half*/
        for (row = 0; row < nr_half_row; row++)
        {
            if (_gps[row][col].is_valid())
            {
                continue;
            }
            const Voxel<DATA_TYPE> &voxel = vg(row, col);
            nr_points_voxel = voxel.nr_points();
            if (!nr_points_voxel)
            {
                continue; /*empty voxel*/
            }
            /*check if the right neighbor is valid*/
            const GroundPlane<DATA_TYPE> &plane = _gps[row + 1][col];
            if (!plane.is_valid())
            {
                continue;
            }
            for (i = 0; i < nr_points_voxel; ++i)
            {
                j = (unsigned int)voxel._indices[i];
                assert(j < nr_points);
                cptr_pc = pc + (nr_point_element * j);
                pt_to_plane_dist_signed =
                    i_plane_to_point_signed_distance_w_unit_norm(plane.params, cptr_pc);
                _signed_ground_dists_in_mm[j] = 
                    (short)i_min((int)(pt_to_plane_dist_signed * (1e+3)),
                    (int)_signed_ground_dists_in_mm[j]);
            }
        }

        /*bottom half*/
        for (row = _nr_gpfit_voxels_vertical - 1; row >= nr_half_row; row--)
        {
            if (_gps[row][col].is_valid())
            {
                continue;
            }
            const Voxel<DATA_TYPE> &voxel = vg(row, col);
            nr_points_voxel = voxel.nr_points();
            if (!nr_points_voxel)
            {
                continue; /*empty voxel*/
            }
            /*check if the left neighbor is valid*/
            const GroundPlane<DATA_TYPE> &plane = _gps[row - 1][col];
            if (!plane.is_valid())
            {
                continue;
            }
            for (i = 0; i < nr_points_voxel; ++i)
            {
                j = (unsigned int)voxel._indices[i];
                assert(j < nr_points);
                cptr_pc = pc + (nr_point_element * j);
                pt_to_plane_dist_signed =
                    i_plane_to_point_signed_distance_w_unit_norm(plane.params, cptr_pc);
                _signed_ground_dists_in_mm[j] = 
                    (short)i_min((int)(pt_to_plane_dist_signed * (1e+3)),
                    (int)_signed_ground_dists_in_mm[j]);
            }
        }
    }
    
    /*added by Liang on 09/28/16*/
    template <typename DATA_TYPE>
    void MultiScaleGroundPlaneDetector<DATA_TYPE>::calc_signed_ground_dist(
        const VoxelGridXY<DATA_TYPE>& vg,
        const DATA_TYPE* pc,
        unsigned int nr_points,
        unsigned int nr_point_element)
    {
        unsigned int row, col;
        for (row = 0; row < _nr_gpfit_voxels_vertical; row++)
        {
            calc_signed_ground_dist_row(vg, pc, nr_points, nr_point_element, row);
        }
        for (col = 0; col < _nr_gpfit_voxels_horizontal; col++)
        {
            calc_signed_ground_dist_col(vg, pc, nr_points, nr_point_element, col);
        }
    }

    /* add by Fangzhen Li on 03/27/17 */
    template <typename DATA_TYPE>
    bool MultiScaleGroundPlaneDetector<DATA_TYPE>::get_voxel_ground_parems(
            const VoxelGridXYPyramid<DATA_TYPE>& vgp,
            const DATA_TYPE *pt,
            unsigned int nr_point_element,
            DATA_TYPE *params) {
        if (!pt || !params || nr_point_element < 2) {
            return false;
        }

        unsigned int nr_scale = vgp.nr_scale();
        int row, col;
        if (!(vgp[nr_scale - 1]).get_voxel_coordinate_xy(pt[0], pt[1], &row, &col)) {
            return false;
        }
        if (!_gps[row][col].is_valid()) {
            return false;
        }
        i_copy4(_gps[row][col].params, params);

        return true;
    }

    template <typename DATA_TYPE>
    int MultiScaleGroundPlaneDetector<DATA_TYPE>::detect(const VoxelGridXYPyramid<DATA_TYPE>& vgp,
                                                         DATA_TYPE upper_bound_z, 
                                                         DATA_TYPE lower_bound_z)
    {
        unsigned int nr_points = vgp.nr_points();

        if (nr_points > _nr_points_max)
        {
            if (reallocate_data(nr_points) != nr_points)
            {
                return (-1);
            }
        }

        if (!_labels || !_signed_ground_dists_in_mm)
        {
            return (-1);
        }
        else
        {
            /*reset buffers*/
            memset((void*)_labels, 0, nr_points*sizeof(char));
            /*for speed consideration using memset to fill short array with a large value,
            value(0111111101111111) = 32639*/
            memset((void*)_signed_ground_dists_in_mm, 127, nr_points*sizeof(short));
        }

        if (!vgp.initialized())
        {
            return (-1);
        }

        unsigned int nr_scale = vgp.nr_scale();

        if (!nr_scale)
        {
            return (-1);
        }
        
        /*handle single scale ground estimation*/
        if (nr_scale == 1)
        {
            return (0); 
        }

        unsigned int i, s = nr_scale - 1;
        unsigned int nr_voxels_vertical = vgp[s].nr_voxel_y();
        unsigned int nr_voxels_horizontal = vgp[s].nr_voxel_x();

        /*added by Liang on 09/28/16*/
        if (!check_gps_buffer_dims(nr_voxels_horizontal, nr_voxels_vertical))
        {
            return (-1);
        }

        /*pre-filter*/
        double z_diff_tol = (double)(_max_z_dim_diff_ground_candidates) * 0.001;

        if (filter_voxelgrid(vgp[0], upper_bound_z, lower_bound_z, (DATA_TYPE)(z_diff_tol)) < 0)
        {
            return (-1);
        }

        int nr_gpts = 0;

        for (i = 0; i < vgp[s].nr_voxel(); ++i)
        {
            nr_gpts += fit_voxel(vgp[s][i],
                vgp.const_data(),
                _gps[0][i],
                vgp.nr_points(),
                vgp.nr_point_element(),
                upper_bound_z,
                lower_bound_z,
                (DATA_TYPE)_dist_tol);
        }

        /*added by Liang on 09/28/16*/
        calc_signed_ground_dist(vgp[s], vgp.const_data(), vgp.nr_points(), vgp.nr_point_element());
        return (nr_gpts);
    }

}/*namespace idl*/

#endif
